/**
 *  基于turtlesim的tf广播器
*/
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

class TurtleTfBroadcaster {
 public:
  TurtleTfBroadcaster(std::string turtle_name) {
    turtle_name_ = turtle_name;
    sub_ = nh_.subscribe(turtle_name+"/pose", 10, &TurtleTfBroadcaster::TurtlePoseCallback, this);
  }

  ~TurtleTfBroadcaster() {

  }

 private:
  ros::NodeHandle nh_;
  ros::Subscriber sub_;
  tf::TransformBroadcaster br_;
  std::string turtle_name_;

  void TurtlePoseCallback(const turtlesim::PoseConstPtr& msg) {
    // 初始化tf数据
    tf::Transform trans;
    trans.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));  // 相对位置
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);                        //  相对姿态
    trans.setRotation(q);
    // 广播world与turtle 坐标系之间的tf数据
    br_.sendTransform(tf::StampedTransform(trans, ros::Time::now(), "world", turtle_name_));
  }
};


int main(int argc, char** argv) {
  ros::init(argc, argv, "demo_tf_broadcaster");

  if (argc != 2) {
    ROS_ERROR("need turtle name as arguments.");
    return -1;
  }
  
  std::string arg1(argv[1]);
  
  TurtleTfBroadcaster turtle_tf_br(arg1);

  ros::spin();

  return 0;
}
